import os

# launch文件中出现的argument和parameter，
# 虽都译为“参数”，但含义不同： - argument：仅限launch文件内部使用，方便在launch中调用某些数值； 
# - parameter：ROS系统的参数，方便在节点见使用某些数值。


# 获取功能包下share目录路径
from ament_index_python.packages import get_package_share_directory # 查询功能包路径的方法
# 参数声明与获取
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch.actions import DeclareLaunchArgument       # 声明launch文件内使用的Argument类

from launch import LaunchDescription    # launch文件的描述类
from launch_ros.actions import Node     # 节点启动的描述类

# 封装终端指令相关类
from launch.actions import ExecuteProcess
from launch.substitutions import FindExecutable

# 事件相关
from launch.event_handlers import OnProcessStart, OnProcessExit,OnExecutionComplete,OnProcessIO,OnShutdown
from launch.actions import ExecuteProcess, RegisterEventHandler, LogInfo



def generate_launch_description():      # 自动生成launch文件的函数
    
    rviz_config = os.path.join(          # 找到配置文件的完整路径
        get_package_share_directory('motion_controller'),
        'rviz',
        # 'display_Astar_search.rviz'
        'motion_controller.rviz'
    )

    # 启动文件即ros2 launch 或者 ros2 run中未显式指定参数,则值为默认值
    # search_choice_arg = DeclareLaunchArgument("search_choice", default_value=["astar"])     # 创建一个Launch文件内参数（arg）background_r

    rviz2 = Node (
        package="rviz2", 
        executable="rviz2",
        name="rviz2", 
        output="screen", 
        # 命令行中后面跟的参数
        # 加载命令行参数
        arguments=['-d', rviz_config]
    ) 
    rqt_graph = Node (
        package="rqt_graph",
        executable="rqt_graph",
        name="rqt_graph"
    )
    
    # astar_gridmap_searcher = Node (
    #     package="astar_gridmap_searcher",
    #     executable="astar_gridmap_searcher",
    #     name="astar_gridmap_searcher",
    #     output="screen",
    #     # ros2节点中的参数param
    #     parameters=[param_config,
    #         # {
    #         #     # 创建参数
    #         #     "search_choice": LaunchConfiguration("search_choice")
    #         # }
    #         ],
    #     # prefix=['xterm -e gdb -ex run --args']

    # )
    tf2_ros = Node (
        package="tf2_ros",
        executable="static_transform_publisher",
        name="odom_combine_broadcaster",
        # <!-- /map /odom: 这两个参数表示这个静态变换是将/odom坐标系（第二个参数）相对于/map坐标系（第一个参数）进行描述。
        # 也就是说，这个静态变换描述了/odom坐标系在/map坐标系中的位置和方向关系。 x y z x y z w四元数 -->
        # 命令行需要的参数需要这样传入 和ros1不一样 不知道的话可以运行一下这个节点，会有提示
        # arguments=["--x","0",
        #            "--y","0",
        #            "--z","0",
        #            "--qx","0",
        #            "--qy","0",
        #            "--qz","0",
        #            "--qw","1",
        #            "--frame-id","map",
        #            "--child-frame-id","base_link",
        #            ]
        arguments=['0', '0', '0', '0', '0', '0', 'map', 'base_link']
    )
    # nav2_map_server = Node (
    #     package="nav2_map_server",
    #     executable="map_server",
    #     # arguments=[map_config]
    #     parameters=[{
    #         "frame_id": "map",
    #         "yaml_filename" : robocon2024_map_config
    #     }
    #     ]
    # )
    # # 代价地图节点
    # costmap2d_node = Node (
    #     package="nav2_costmap_2d",
    #     executable="nav2_costmap_2d",
    #     output="screen",
    #     parameters=[robocon2024_costmap_config],
    #     respawn=True        #自动重启节点
    # )



    # # ros1与ros2的大不同，多了个生命周期节点 参考旭哥
    # # 配置环境 --configure --activate nav_map_server这个功能包里面的节点有启动的先后顺序！
    # configure = ExecuteProcess(
    #     cmd=[FindExecutable(name="ros2"),' lifecycle ','set ','/map_server ','configure'],
    #     shell=True
    # )
    # activate = ExecuteProcess(
    #     cmd=[FindExecutable(name="ros2"),' lifecycle ','set ','/map_server ','activate'],
    #     shell=True
    # )

    # costmap2d_configure = ExecuteProcess(
    #     cmd=[FindExecutable(name="ros2"),' lifecycle ','set ','/costmap/costmap ','configure'],
    #     shell=True
    # )
    # costmap2d_activate = ExecuteProcess(
    #     cmd=[FindExecutable(name="ros2"),' lifecycle ','set ','/costmap/costmap ','activate'],
    #     shell=True
    # )

    # # 事件发生器 ---先打开rviz，然后进行config，然后进行activa
    # # 先开启并激活mapserver再启动costmap_2d
    # configure_event = RegisterEventHandler(
    #     event_handler=OnProcessStart(
    #         target_action=nav2_map_server,
    #         on_start=configure                     
    #     )
    # )
    # activate_event = RegisterEventHandler(
    #     event_handler=OnExecutionComplete(
    #         target_action=configure,
    #         on_completion=[
    #             LogInfo(msg='***********map_server configure finish'),
    #             activate
    #         ]
    #     )
    # )
    # # 代价地图的事件发生器
    # costmap2d_configure_event = RegisterEventHandler(
    #     event_handler=OnProcessStart(
    #         target_action=activate,
    #         on_start=costmap2d_configure                     
    #     )
    # )
    # costmap2d_activate_event = RegisterEventHandler(
    #     event_handler=OnExecutionComplete(
    #         target_action=costmap2d_configure,
    #         on_completion=[
    #             LogInfo(msg='***********costmap2d configure finish,next is costmap2d_activate'),
    #             costmap2d_activate
    #         ]
    #     )
    # )


    return LaunchDescription([
        # search_choice_arg,
        # nav2_map_server,
        # costmap2d_node,
        tf2_ros,
        rviz2
        # astar_gridmap_searcher,
        # rqt_graph,
        # configure_event,
        # activate_event,
        # costmap2d_configure_event,
        # costmap2d_activate_event
        ]
    )